import rospy
from controller import Controller

robot = Controller('go1')
print(robot.robot_name)
robot.motion_init()


# array of motors angles for different poses
# sitting
pos1 = [0.0, 0.67, -1.3,
        -0.0, 0.67, -1.3,
        -0.2, 1.2, -2.2,
        0.2, 1.2, -2.2]

# front right leg up
pos2 = {0.0, 0.0, -3,
        -0.0, 0.67, -1.3,
        -0.2, 1.2, -2.2,
        0.2, 1.2, -2.2}

# front right leg up hip up
pos3 = {-0.3, 0.0, -3,
        -0.0, 0.67, -1.3,
        -0.2, 1.2, -2.2,
        0.2, 1.2, -2.2}

# front right leg up hip down
pos4 = {0.3, 0.0, -3,
        -0.0, 0.67, -1.3,
        -0.2, 1.2, -2.2,
        0.2, 1.2, -2.2}

# front right leg up hip straight
pos5 = {0.0, 0.0, -3,
        -0.0, 0.67, -1.3,
        -0.2, 1.2, -2.2,
        0.2, 1.2, -2.2}

# front right leg up forward
pos6 = {0.0, -0.5, -2,
        -0.0, 0.67, -1.3,
        -0.2, 1.2, -2.2,
        0.2, 1.2, -2.2}

# sitting
pos7 = {0.0, 0.67, -1.3,
        -0.0, 0.67, -1.3,
        -0.2, 1.2, -2.2,
        0.2, 1.2, -2.2}

# high sitting
pos8 = {0.0, 1.0, -0.5,
        -0.0, 1.0, -0.5,
        -0.2, 1.0, -1.8,
        0.2, 1.0, -1.8}
# stay
pos9 = {0.0, 0.67, -1.3,
        -0.0, 0.67, -1.3,
        -0.0, 0.67, -1.3,
        0.0, 0.67, -1.3}

robot.takePosition(pos1, 1000)
robot.takePosition(pos2, 1000)
robot.takePosition(pos3, 1000)
robot.takePosition(pos4, 1000)
robot.takePosition(pos5, 1000)
robot.takePosition(pos6, 1000)
robot.takePosition(pos7, 1000)
robot.takePosition(pos8, 1000)
robot.takePosition(pos9, 1000)


rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
    rospy.spin()